/* @file monitor.h
 * @brief This file contains the declaration of the Monitor class.
 *
 * This file provides the declaration of the Monitor class, which is used to
 * monitoring base class. The class definition includes a constructor and
 * some member functions.
 *
 * @author haibo.yang
 * @date 2023-06-09
 */
#pragma once

#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/logger.hpp"
#include <sys/types.h>
#include <sys/syscall.h>
#include <unistd.h>
#include <boost/thread/thread.hpp> 
#include <boost/bind/bind.hpp> 
#include <agv_msgs/msg/agv_event.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>

#define USE_ROS_CONSOLE

namespace monitor {
#ifdef USE_ROS_CONSOLE
 
#define MONITOR_DEBUG(...)  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("monitor"), __VA_ARGS__)
#define MONITOR_INFO(...)   RCLCPP_INFO_STREAM(rclcpp::get_logger("monitor"), __VA_ARGS__)
#define MONITOR_WARN(...)   RCLCPP_WARN_STREAM(rclcpp::get_logger("monitor"), __VA_ARGS__)
#define MONITOR_ERROR(...)  RCLCPP_ERROR_STREAM(rclcpp::get_logger("monitor"), __VA_ARGS__)

#define ASSERT(condition)   assert(condition)
#else

#define MONITOR_DEBUG(...)  std::cout << "[Debug]: " << __VA_ARGS__ << std::endl
#define MONITOR_INFO(...)   std::cout << "[Info]: " << __VA_ARGS__ << std::endl
#define MONITOR_WARN(...)   std::cout << "[Warn]: " << __VA_ARGS__ << std::endl
#define MONITOR_ERROR(...)  std::cout << "[Error]: " << __VA_ARGS__ << std::endl

#define ASSERT(condition)   assert(condition)
#endif

typedef std::pair<rclcpp::Time, agv_msgs::msg::AGVEvent> AgvEvent; // single event when one node publish
typedef std::vector<AgvEvent> AgvEventVector;              // multiple events, one node publishes a collection
typedef std::map<std::string, AgvEventVector> AgvEventMap; // multiple events, multiple node publishes a collection

enum Mode
{
    Mode_Manul = 0,
    Mode_Semi_Auto = 1,
    Mode_Local_Auto = 2,
    Mode_Master_Auto = 3,
    Mode_Idle = 4
};

enum SafetyState {
    NORMAL = 0,
    WARN,
    SLOW,
    PAUSE,
    OBSTACLESTOP,
    EVENTSTOP,
    IOTIMEOUT,
    OBSTACLETIMEOUT,
    STOP,
    BUMPERSTOP,
    EMERGENCYSTOP,
};

class Monitor
{
public:
    explicit Monitor(const std::string &name, const int interval, const bool mode=false):
            name_(name), round_count_(0), interval_(interval), 
            start_time_(Monitor::GetTimes()), 
            next_round_(start_time_),
            sim_mode_(mode) 
    {
        if (!Monitor::init_) {
            agv_msgs::msg::ReadInPutsNew msg;
            Monitor::init_ = true;
            Monitor::inputs.first = Monitor::GetTimes();
            Monitor::inputs.second = msg;
            // subscript io & obstacle 
            io_input_sub_ = Monitor::node_->create_subscription<agv_msgs::msg::ReadInPutsNew>(
                "/readInputNew", 10, 
                std::bind(&Monitor::InputCallBack, this, std::placeholders::_1));
        }
    }

    virtual ~Monitor();

    // Tick once, which may or may not execute the RunOnce() function, based on
    // the interval setting.
    void Tick(const rclcpp::Time current_time);

    // read input msg callback 
    void InputCallBack(const agv_msgs::msg::ReadInPutsNew &msg);

    // init class params
    virtual void init() = 0;

    // Do the actual work.
    virtual void RunOnce(const rclcpp::Time current_time) = 0;
    
    static bool init_parameters();
    
    static std::string GetVehicleModel() {
        return vehicle_model_;
    }

    static void SetVehicleModel(const std::string model) {
        vehicle_model_ = model;
        MONITOR_INFO("Set vehicle model:" << model);
    }

    static int GetVehicleSaftyState() {
        return vehicle_safty_state_;
    }

    static void SetVehicleSaftyState(const int state) {
        if (vehicle_safty_state_ != state)
            MONITOR_INFO("Set new vehicle state:" << state);
        vehicle_safty_state_ = state;
    }

    static int GetVehicleMode() {
        return vehicle_mode_;
    }

    static void SetVehicleMode(const int mode) {
        vehicle_mode_ = mode;
        MONITOR_INFO("Set vehicle mode:" << mode);
    }

    static rclcpp::Time GetTimes(void) {
        return Monitor::node_->now();
    }

    static std::shared_ptr<rclcpp::Node> GetNode(void) {
        return Monitor::node_;
    }
    
    static std::pair<rclcpp::Time, agv_msgs::msg::ReadInPutsNew> inputs; 
    static std::shared_ptr<rclcpp::Node> node_;
    static bool init_;
protected:
    std::string name_;
    unsigned int round_count_;
    rclcpp::Time start_time_;
    rclcpp::Time next_round_;
    bool sim_mode_ = false;

private:
    double interval_;
    rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr io_input_sub_;

    static std::string vehicle_model_;
    static int vehicle_safty_state_;
    static int vehicle_mode_;
};

}  // namespace monitor
